/*
 * mtiguide.c
 *
 *  Created on: 2010-1-27
 *      Author: Administrator
 */

#include <leaf/base.h>
#include <leaf/walkspeed.h>
#include "mtiguide.h"
signed short angle1=0;
Angle currentAngle=0;
static Angle   direction = 0;
static int    kp = 500;
static int    kd = 200;
static int diver = 5000;
static int maxSpeed = 5000;
static int minSpeed = 300;
static Angle   offsets[3] = {0, 0, 0};
SpeedPair curSpeed={0,0};



static int calcAdjustment( void ){
	int adjustment = maxSpeed;
	adjustment *= (
			kp * offsets[2]
			             +   kd * ( offsets[2] - 2 * offsets[1] + offsets[0] )
	);
	adjustment /= diver;
	return ABS( adjustment );
}


SpeedPair adjustSpeedPos(SpeedPair *curSpeed ){
	SpeedPair calcSpeed = {maxSpeed, maxSpeed};
	int lastAdjustment = 0;
	int offset = offsets[2];
	if( absint( offset ) > 7 )
		lastAdjustment = absint( absint(curSpeed->left) - absint(curSpeed->right) );
	if( offset > 0 ){
		if(offset > 10){
			calcSpeed.left =- minSpeed;
		}
		else{
			calcSpeed.left =(-maxSpeed) +lastAdjustment + calcAdjustment( );
			if(ABS(calcSpeed.left) < ABS(minSpeed) || calcSpeed.left>0){
				calcSpeed.left =- minSpeed;
			}
		}
		calcSpeed.right = maxSpeed;
	}
	else{
		if( offset < -10 ){
			calcSpeed.right = minSpeed;
		}
		else{
			calcSpeed.right = maxSpeed - lastAdjustment - calcAdjustment( );
			if(calcSpeed.right < minSpeed){
				calcSpeed.right = minSpeed;
			}
		}
		calcSpeed.left = -maxSpeed;
	}
	return calcSpeed;
}


SpeedPair adjustSpeed(SpeedPair *cs )
{
	if(maxSpeed == 0)
		return ( (SpeedPair){0, 0} );
	if( absint(offsets[2]) < 2 ){
		return (SpeedPair){-maxSpeed, maxSpeed};
	}
	if(maxSpeed > 0)
		return adjustSpeedPos( cs );
	else
		return ( (SpeedPair){ -maxSpeed, maxSpeed } );
}



Angle adjustAngle(Angle a){
	Angle ret = a;
	while( ret >= 180 )
		ret -= 360;
	while( ret < -180 )
		ret += 360;
	return ret;
}


static void getSample( Angle curangle )
{
	Angle offset = adjustAngle( direction - curangle );
	offsets[0] = offsets[1];
	offsets[1] = offsets[2];
	offsets[2] = offset;
}


void mtiGuide(Angle curAngle)
{
    getSample(curAngle);
    curSpeed=adjustSpeed(&curSpeed);
    if(absint(curSpeed.left)<=maxSpeed && absint(curSpeed.right)<=maxSpeed)
    {
        setLeftSpeed(curSpeed.left);
        setRightSpeed(curSpeed.right);
    }
    else
    {
        setLeftSpeed(maxSpeed);
        setRightSpeed(maxSpeed);
    }
}
